﻿using System;
using System.Windows;
using System.Windows.Media;
using System.Threading;
using KinectAPI;
using System.ComponentModel;
using System.Windows.Controls;
using System.Windows.Shapes;
using xn;
using System.Collections.Generic;
using Emgu.CV;
using Emgu.CV.Structure;
using HandGestureRecognition.SkinDetector;
using System.Windows.Media.Imaging;
using System.Runtime.InteropServices;
using System.IO;

namespace KinectApp
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {

        #region Fields

        protected HandTracker _handTracker;
        BackgroundWorker _bgWorker1 = new BackgroundWorker();
        BackgroundWorker _bgWorker2 = new BackgroundWorker();
        int IMAGE_WIDTH;
        int IMAGE_HEIGHT;
        private BodyTracker _skeleton;
        private BodyGestureGenerator _gestureGenerator;
        Color[] _userColors = { Colors.Blue, Colors.Red, Colors.Green, Colors.Yellow, Colors.Pink, Colors.Black };
        int modifColor = 0;
        LinkedList<PositionEventArgs> point;
        bool modeVolant = false;
        bool noSkeleton = false;
        bool tour = true;
        private Thread thread = null;


        /*********/
        IColorSkinDetector skinDetector;

        Image<Bgr, Byte> currentFrame;
        Image<Bgr, Byte> currentFrameCopy;

        Capture grabber;
        AdaptiveSkinDetector detector;

        int frameWidth;
        int frameHeight;

        Hsv hsv_min;
        Hsv hsv_max;
        Ycc YCrCb_min;
        Ycc YCrCb_max;

        Seq<System.Drawing.Point> hull;
        MCvBox2D box;
        Emgu.CV.Structure.Ellipse ellip;
        private Image<Gray, byte> skin;
        private System.Drawing.Bitmap skin2;



        #endregion

        public MainWindow()
        {
            
            InitializeComponent();

            CompositionTarget.Rendering += new EventHandler(CompositionTarget_Rendering);

            /*********/
            grabber = new Capture(2);
            if (grabber != null) grabber.FlipHorizontal = !grabber.FlipHorizontal;
            IntPtr frame = grabber.QueryFrame();
            frameWidth = grabber.Width;
            frameHeight = grabber.Height;
            detector = new AdaptiveSkinDetector(1, AdaptiveSkinDetector.MorphingMethod.NONE);
            hsv_min = new Hsv(0, 45, 0);
            hsv_max = new Hsv(20, 255, 255);
            YCrCb_min = new Ycc(0, 131, 80);
            YCrCb_max = new Ycc(255, 255, 255);
            box = new MCvBox2D();
            ellip = new Emgu.CV.Structure.Ellipse();



            _handTracker = new HandTracker();
            _handTracker.HandPosition += new HandTracker.HandPositionHandler(HandPosition_Updated);
            _handTracker.Frame += new HandTracker.FrameHandler(FrameGrabber);

            point = new LinkedList<PositionEventArgs>();

            _skeleton = new BodyTracker();
            _skeleton.UsersUpdated += new BodyTracker.UsersUpdatedHandler(Users_Updated);

            _gestureGenerator = new BodyGestureGenerator(_skeleton);
            _gestureGenerator.Add("MoveUp");
            _gestureGenerator.Add("MoveDown");
            _gestureGenerator.Add("MoveRight");
            _gestureGenerator.Add("MoveLeft");
            _gestureGenerator.Add("Volant");
            _gestureGenerator.Add("Dollar", new string[]{"circle", "triangle", "rectangle", "x"});
            _gestureGenerator.Recognised += new BodyGestureGenerator.GestureRecognisedHandler(_gestureGenerator_Recognised);
            _gestureGenerator.Start += new BodyGestureGenerator.GestureStartHandler(_gestureGenerator_Start);
            _gestureGenerator.End += new BodyGestureGenerator.GestureEndHandler(_gestureGenerator_End);

            IMAGE_WIDTH = _skeleton.Width;
            IMAGE_HEIGHT = _skeleton.Height;

            _bgWorker1.DoWork += new DoWorkEventHandler(Worker_DoWork);
            //_bgWorker1.DoWork += new DoWorkEventHandler(Worker_DoWork2);
        }

        #region Events Handlers

        void CompositionTarget_Rendering(object sender, EventArgs e)
        {
            if (!_bgWorker1.IsBusy)
            {
                _bgWorker1.RunWorkerAsync();
            }
        }

        public BitmapImage ImageFromBuffer(Byte[] bytes)
        {
            MemoryStream stream = new MemoryStream(bytes);
            BitmapImage image = new BitmapImage();
            image.BeginInit();
            image.StreamSource = stream;
            image.EndInit();
            return image;
        }

        public byte[] imageToByteArray(System.Drawing.Image imageIn)
        {
            MemoryStream ms = new MemoryStream();
            imageIn.Save(ms, System.Drawing.Imaging.ImageFormat.Gif);
            return ms.ToArray();
        }

        void Worker_DoWork(object sender, DoWorkEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                imgCamera1.Source = _skeleton.RawImage;
                imgCamera2.Source = _skeleton.DepthImage;
                //imgCamera3.Source = ToBitmapSource(grabber.QueryFrame().Bitmap);
            });
        }

        void HandPosition_Updated(object sender, PositionEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                textBoxX.Clear();
                textBoxY.Clear();
                textBoxZ.Clear();

                textBoxX.AppendText(e.X.ToString());
                textBoxY.AppendText(e.Y.ToString());
                textBoxZ.AppendText(e.Z.ToString());

                if (false)
                {
                    if(point.Count == 35)
                    {
                        point.RemoveFirst();
                    }
                    point.AddLast(e);

                    LayoutRoot2.Children.Clear();

                    foreach (PositionEventArgs p in point)
                    {
                        DrawCircle(p.X, p.Y, p.Z, Colors.Green, LayoutRoot2);
                    }
                }
                tour = !tour;

                LayoutRoot3.Children.Clear();
                float diametre = 200 - ((e.Z / 100) * 5);
                if (diametre < 10) diametre = 10;
                Rectangle rect = new Rectangle
                {
                    Height = diametre,
                    Width = diametre,
                    Margin = new Thickness(e.X - (diametre / 2), e.Y - (diametre / 2), 0, 0),
                    Stroke = new SolidColorBrush(Colors.Red),
                    StrokeThickness = 5
                };

                System.Windows.Shapes.Ellipse ellipse = new System.Windows.Shapes.Ellipse
                {
                    Fill = new SolidColorBrush(Colors.Red),
                    Width = diametre,
                    Height = diametre,
                    Margin = new Thickness(e.X - (diametre / 2), e.Y - (diametre / 2), 0, 0)
                };
            });
        }

        void FrameGrabber(object sender, PositionEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                currentFrame = grabber.QueryFrame();
                if (currentFrame != null)
                {
                    float diametre = 200 - ((e.Z / 100) * 5);
                    if (diametre < 10) diametre = 10;

                    MCvBox2D rect = new MCvBox2D(new System.Drawing.PointF(e.X, e.Y), new System.Drawing.SizeF(diametre, diametre), 0);
                    currentFrameCopy = currentFrame.Copy(rect);

                    skinDetector = new YCrCbSkinDetector();

                    skin = skinDetector.DetectSkin(currentFrameCopy, YCrCb_min, YCrCb_max);
                    if (skin != null)
                    {
                        System.Console.WriteLine(skin.Rows);
                        skin2 = skin.ToBitmap();
                        imgCamera3.Source = ToBitmapSource(skin2);
                        //Thread.Sleep(100);
                    }
                    ExtractContourAndHull(skin, e.X, e.Y);

                }
            });
        }

        void Users_Updated(object sender, UsersEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                lock (this)
                {
                    LayoutRoot1.Children.Clear();
                    foreach (var user in e.users)
                    {
                        Color userColor = _userColors[user.Id - 1 + modifColor];

                        if (!modeVolant && !noSkeleton)
                        {

                            #region Get body parts coordinates

                            float headX = user.Head.X;
                            float headY = user.Head.Y;
                            float headZ = user.Head.Z;

                            float neckX = user.Neck.X;
                            float neckY = user.Neck.Y;
                            float neckZ = user.Neck.Z;

                            float leftShoulderX = user.LeftShoulder.X;
                            float leftShoulderY = user.LeftShoulder.Y;
                            float leftShoulderZ = user.LeftShoulder.Z;

                            float leftElbowX = user.LeftElbow.X;
                            float leftElbowY = user.LeftElbow.Y;
                            float leftElbowZ = user.LeftElbow.Z;

                            float leftHandX = user.LeftHand.X;
                            float leftHandY = user.LeftHand.Y;
                            float leftHandZ = user.LeftHand.Z;

                            float rightShoulderX = user.RightShoulder.X;
                            float rightShoulderY = user.RightShoulder.Y;
                            float rightShoulderZ = user.RightShoulder.Z;

                            float rightElbowX = user.RightElbow.X;
                            float rightElbowY = user.RightElbow.Y;
                            float rightElbowZ = user.RightElbow.Z;

                            float rightHandX = user.RightHand.X;
                            float rightHandY = user.RightHand.Y;
                            float rightHandZ = user.RightHand.Z;

                            float waistX = user.Torso.X;
                            float waistY = user.Torso.Y;
                            float waistZ = user.Torso.Z;

                            float leftHipX = user.LeftHip.X;
                            float leftHipY = user.LeftHip.Y;
                            float leftHipZ = user.LeftHip.Z;

                            float leftKneeX = user.LeftKnee.X;
                            float leftKneeY = user.LeftKnee.Y;
                            float leftKneeZ = user.LeftKnee.Z;

                            float leftFootX = user.LeftFoot.X;
                            float leftFootY = user.LeftFoot.Y;
                            float leftFootZ = user.LeftFoot.Z;

                            float rightHipX = user.RightHip.X;
                            float rightHipY = user.RightHip.Y;
                            float rightHipZ = user.RightHip.Z;

                            float rightKneeX = user.RightKnee.X;
                            float rightKneeY = user.RightKnee.Y;
                            float rightKneeZ = user.RightKnee.Z;

                            float rightFootX = user.RightFoot.X;
                            float rightFootY = user.RightFoot.Y;
                            float rightFootZ = user.RightFoot.Z;

                            #endregion

                            #region Draw skeleton points

                            DrawCircle(headX, headY, headZ, userColor, LayoutRoot1);
                            DrawCircle(neckX, neckY, neckZ, userColor, LayoutRoot1);
                            DrawCircle(leftShoulderX, leftShoulderY, leftShoulderZ, userColor, LayoutRoot1);
                            DrawCircle(leftElbowX, leftElbowY, leftElbowZ, userColor, LayoutRoot1);
                            if (leftHandY < headY)
                            {
                                DrawCircle(leftHandX, leftHandY, leftHandZ, Colors.Chocolate, LayoutRoot1);
                            }
                            else
                            {
                                DrawCircle(leftHandX, leftHandY, leftHandZ, userColor, LayoutRoot1);
                            }
                            DrawCircle(waistX, waistY, waistZ, userColor, LayoutRoot1);
                            DrawCircle(rightShoulderX, rightShoulderY, rightShoulderZ, userColor, LayoutRoot1);
                            DrawCircle(rightElbowX, rightElbowY, rightElbowZ, userColor, LayoutRoot1);
                            if (rightHandY < headY)
                            {
                                DrawCircle(rightHandX, rightHandY, rightHandZ, Colors.Chocolate, LayoutRoot1);
                            }
                            else
                            {
                                DrawCircle(rightHandX, rightHandY, rightHandZ, userColor, LayoutRoot1);
                            }
                            DrawCircle(leftHipX, leftHipY, leftHipZ, userColor, LayoutRoot1);
                            DrawCircle(leftKneeX, leftKneeY, leftKneeZ, userColor, LayoutRoot1);
                            DrawCircle(leftFootX, leftFootY, leftFootZ, userColor, LayoutRoot1);
                            DrawCircle(rightHipX, rightHipY, rightHipZ, userColor, LayoutRoot1);
                            DrawCircle(rightKneeX, rightKneeY, rightKneeZ, userColor, LayoutRoot1);
                            DrawCircle(rightFootX, rightFootY, rightFootZ, userColor, LayoutRoot1);

                            #endregion

                            #region Draw skeleton line

                            DrawLine(headX, headY, neckX, neckY, userColor, LayoutRoot1);
                            DrawLine(neckX, neckY, leftShoulderX, leftShoulderY, userColor, LayoutRoot1);
                            DrawLine(neckX, neckY, rightShoulderX, rightShoulderY, userColor, LayoutRoot1);
                            DrawLine(leftShoulderX, leftShoulderY, leftElbowX, leftElbowY, userColor, LayoutRoot1);
                            DrawLine(leftElbowX, leftElbowY, leftHandX, leftHandY, userColor, LayoutRoot1);
                            DrawLine(neckX, neckY, waistX, waistY, userColor, LayoutRoot1);
                            DrawLine(leftShoulderX, leftShoulderY, waistX, waistY, userColor, LayoutRoot1);
                            DrawLine(rightShoulderX, rightShoulderY, waistX, waistY, userColor, LayoutRoot1);
                            DrawLine(rightShoulderX, rightShoulderY, rightElbowX, rightElbowY, userColor, LayoutRoot1);
                            DrawLine(rightElbowX, rightElbowY, rightHandX, rightHandY, userColor, LayoutRoot1);
                            DrawLine(waistX, waistY, leftHipX, leftHipY, userColor, LayoutRoot1);
                            DrawLine(waistX, waistY, rightHipX, rightHipY, userColor, LayoutRoot1);
                            DrawLine(leftHipX, leftHipY, rightHipX, rightHipY, userColor, LayoutRoot1);
                            DrawLine(leftHipX, leftHipY, leftKneeX, leftKneeY, userColor, LayoutRoot1);
                            DrawLine(leftKneeX, leftKneeY, leftFootX, leftFootY, userColor, LayoutRoot1);
                            DrawLine(rightHipX, rightHipY, rightKneeX, rightKneeY, userColor, LayoutRoot1);
                            DrawLine(rightKneeX, rightKneeY, rightFootX, rightFootY, userColor, LayoutRoot1);

                            #endregion

                        }
                        else if (modeVolant)
                        {
                            DrawLine(user.LeftHand.X, user.LeftHand.Y, user.RightHand.X, user.RightHand.Y, userColor, LayoutRoot1);
                        }
                    }
                }
            });
        }

        void _gestureGenerator_Recognised(string gesture, object[] args)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                switch (gesture)
                {
                    case "MoveUp":
                        tabControl1.SelectedIndex = (tabControl1.SelectedIndex + 1) % 2;
                        break;
                    case "MoveDown":
                        tabControl1.SelectedIndex = (tabControl1.SelectedIndex + 1) % 2;
                        break;
                    case "MoveRight":
                        modifColor = (modifColor + 1) % _userColors.Length;
                        break;
                    case "MoveLeft":
                        modifColor = (modifColor + 1) % _userColors.Length;
                        break;
                    case "Volant":
                        LayoutRoot3.Children.Clear();
                        Line line = new Line
                        {
                            X1 = 0,
                            Y1 = 0,
                            X2 = 30,
                            Y2 = 0,
                            Margin = new Thickness(30, 30, 0, 0),
                            Stroke = new SolidColorBrush(Colors.BlueViolet),
                            StrokeThickness = 4
                        };
                        line.RenderTransform = new RotateTransform
                        {
                            Angle = (double)args[0]
                        };
                        System.Windows.Shapes.Ellipse ellipse = new System.Windows.Shapes.Ellipse
                        {
                            Width = 60,
                            Height = 60,
                            Margin = new Thickness(0, 0, 0, 0),
                            Stroke = new SolidColorBrush(Colors.BlueViolet),
                            StrokeThickness = 4
                        };
                        LayoutRoot3.Children.Add(ellipse);
                        LayoutRoot3.Children.Add(line);
                        break;
                    case "Dollar":
                        System.Console.WriteLine("Recognised Dollar : " + args[0] + " avec un score de " + args[1]);
                        System.Console.WriteLine(((string)args[0]).Substring(0, ((string)args[0]).Length - 2));
                        point.Clear();
                        break;
                }
            });
        }

        void _gestureGenerator_Start(string gesture, object[] args)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                switch (gesture)
                {
                    case "Volant":
                        modeVolant = true;
                        break;
                }
            });
        }

        void _gestureGenerator_End(string gesture, object[] args)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                switch (gesture)
                {
                    case "Volant":
                        modeVolant = false;
                        LayoutRoot3.Children.Clear();
                        break;
                }
            });
        }

        #endregion

        #region Private Methods

        void DrawCircle(float positionX, float positionY, float positionZ, Color color, Canvas layout)
        {
            float diametre = 120 - ((positionZ / 100) * 5);

            System.Windows.Shapes.Ellipse ellipse = new System.Windows.Shapes.Ellipse
            {
                Fill = new SolidColorBrush(color),
                Width = diametre,
                Height = diametre,
                Margin = new Thickness(positionX - (diametre / 2), positionY - (diametre / 2), 0, 0)
            };
            layout.Children.Add(ellipse);
        }

        void DrawLine(float positionX1, float positionY1, float positionX2, float positionY2, Color color, Canvas layout)
        {
            Line line = new Line
            {
                X1 = positionX1,
                Y1 = positionY1,
                X2 = positionX2,
                Y2 = positionY2,
                Stroke = new SolidColorBrush(color),
                StrokeThickness = 4
            };
            layout.Children.Add(line);
        }

        #endregion



        /// <summary>
        /// Delete a GDI object
        /// </summary>
        /// <param name="o">The pointer to the GDI object to be deleted</param>
        /// <returns></returns>
        [DllImport("gdi32")]
        private static extern int DeleteObject(IntPtr o);

        /// <summary>
        /// Convert an IImage to a WPF BitmapSource. The result can be used in the Set Property of Image.Source
        /// </summary>
        /// <param name="image">The Emgu CV Image</param>
        /// <returns>The equivalent BitmapSource</returns>
        public static BitmapSource ToBitmapSource(System.Drawing.Bitmap source)
        {
            IntPtr ptr = source.GetHbitmap(); //obtain the Hbitmap

            BitmapSource bs = System.Windows.Interop.Imaging.CreateBitmapSourceFromHBitmap(
                ptr,
                IntPtr.Zero,
                Int32Rect.Empty,
                System.Windows.Media.Imaging.BitmapSizeOptions.FromEmptyOptions());

            DeleteObject(ptr); //release the HBitmap
            return bs;
        }

        private void ExtractContourAndHull(Image<Gray, byte> skin, float X, float Y)
        {
            using (MemStorage storage = new MemStorage())
            {
                Contour<System.Drawing.Point> contours =
                    skin.FindContours(Emgu.CV.CvEnum.CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, Emgu.CV.CvEnum.RETR_TYPE.CV_RETR_LIST, storage);
                Contour<System.Drawing.Point> biggestContour = null;

                Double Result1 = 0;
                Double Result2 = 0;
                while (contours != null)
                {
                    Result1 = contours.Area;
                    if (Result1 > Result2)
                    {
                        Result2 = Result1;
                        biggestContour = contours;
                    }
                    contours = contours.HNext;
                }

                if (biggestContour != null)
                {
                    Contour<System.Drawing.Point> currentContour = biggestContour.ApproxPoly(biggestContour.Perimeter * 0.0025, storage);
                    biggestContour = currentContour;


                    hull = biggestContour.GetConvexHull(Emgu.CV.CvEnum.ORIENTATION.CV_CLOCKWISE);
                    box = biggestContour.GetMinAreaRect();
                    System.Drawing.PointF[] points = box.GetVertices();

                    Point[] ps = new Point[points.Length];
                    for (int i = 0; i < points.Length; i++)
                        ps[i] = new Point((int)points[i].X, (int)points[i].Y);

                    // Dessin de la Hull
                    System.Windows.Media.PointCollection polygonPoints = new System.Windows.Media.PointCollection();
                    foreach (System.Drawing.Point hp in hull.ToArray())
                    {
                        Point p = new Point(X - (box.center.X / 2) + Convert.ToInt32(hp.X), Y - (box.center.Y / 2) + Convert.ToInt32(hp.Y));
                        polygonPoints.Add(p);
                    }
                    System.Drawing.Point hp0 = hull.ToArray()[0];
                    Point p0 = new Point(X - box.center.X + Convert.ToInt32(hp0.X), Y - box.center.Y + Convert.ToInt32(hp0.Y));
                    polygonPoints.Add(p0);
                    Polyline yellowPolyline = new Polyline();
                    yellowPolyline.Stroke = new SolidColorBrush(Colors.Aquamarine);
                    yellowPolyline.StrokeThickness = 4;
                    yellowPolyline.Points = polygonPoints;
                    LayoutRoot3.Children.Add(yellowPolyline);

                    // Dessin du centre
                    System.Windows.Shapes.Ellipse ellipse = new System.Windows.Shapes.Ellipse
                    {
                        Fill = new SolidColorBrush(Colors.Aquamarine),
                        Width = 4,
                        Height = 4,
                        Margin = new Thickness(X - (box.center.X / 2) - (4 / 2), Y - (box.center.Y / 2) - (4 / 2), 0, 0)
                    };
                    LayoutRoot3.Children.Add(ellipse);
                    //currentFrame.Draw(new CircleF(new System.Drawing.PointF(box.center.X, box.center.Y), 3), new Bgr(200, 125, 75), 2);
                }
            }
        }

    }
}
